#include "axis_settings_cmd.h"
#include "log/zuclog.h"
#include "errcode.h"
#include "safety/mot/safety_recheck.h"
//#include "tp/tpmanager.hh"
#include "dyna/jaka_dyn_intf.h"
#include "motion/motion_debug.h"

#include <string.h>

#define MOTCMD_INFO(fmt, ...) zuclog_info("AXISCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_ERR(fmt, ...) zuclog_error("AXISCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_DEBUG(fmt, ...) zuclog_debug("AXISCMD", "[%06d] " fmt, __LINE__, ##__VA_ARGS__)

int CmdStep::process()
{
    /* resume paused motion until id changes */
    /* can happen at any time */
    MOTCMD_DEBUG("STEP");
    // if (zucmotStatus->paused)
    // {
    //     zucmotDebug->idForStep = zucmotStatus->id;
    //     zucmotDebug->stepping = 1;
    //     // MOTCMD_INFO("2: zucmotDebug->stepping set to 1\n");
    //    //     tpManager_tpresume();
    //     zucmotStatus->paused = 1;
    // }
    // else
    // {
    //     reportError(CANNOT_STEP_WHILE_RUNNING, "MOTION: can't STEP while already executing");
    // }
    return 0;
}

int CmdFeedScale::process()
{
    MOTCMD_DEBUG("FEED SCALE");
    if (scale_ < 0.0)
    {
        scale_ = 0.0; /* clamp it */
    }
    // zucmotStatus->feed_scale = scale_;
    return 0;
}
